/******************************************************************************
 *          Devantech SRF10 Sonar Ranger Functions (implementation)
 ******************************************************************************/
 
#include "twi-utils.h"

int16_t sonarRead(uint8_t address)
{
    uint8_t buffer[2];
    int16_t res;

    buffer[0] = 0;
    buffer[1] = 0;

    twiStart();
    twiSelect(address | TW_WRITE);

    twiWrite(0);

    twiStart();  // send (repeat) start condition
    twiSelect(address | TW_READ);
    twiRead(buffer, 0);
	
	if(buffer[0] != 255)
	{
        // done ranging, read the results
        twiStart();
        twiSelect(address | TW_WRITE);
        twiWrite(2);
        
        twiStart();
        twiSelect(address | TW_READ);
    
        twiRead(buffer, 1);
        twiRead(buffer+1, 0);

        res = (buffer[0]<<8) | buffer[1];
	}
	else {
		res = -1; // no data available yet
    }

    twiStop();
	return res;
}

void sonarStart(uint8_t address)
{
    twiStart();
    twiSelect(address | TW_WRITE);
    twiWrite(0);
    twiWrite(81); // measure in centimeters. Write 80 to get result in inches
    twiStop();
}

void sonarChangeAddress(uint8_t originalAddress, uint8_t newAddress)
{
    twiStart();
    twiSelect(originalAddress | TW_WRITE);
    twiWrite(0);
    twiWrite(0xA0);

    twiStart();
    twiSelect(originalAddress | TW_WRITE);
    twiWrite(0);
    twiWrite(0xAA);

    twiStart();
    twiSelect(originalAddress | TW_WRITE);
    twiWrite(0);
    twiWrite(0xA5);

    twiStart();
    twiSelect(originalAddress | TW_WRITE);
    twiWrite(0);
    twiWrite(newAddress);

    twiStop();
}
